/*
 * File:         CR1.cpp
 * Date:         2011 03 29
 * Description:  Controller for Robot type 1
 * Author:       Gonzalo R. Canosa
 *
 *
 */

#include <webots/Robot.hpp>
#include <webots/Node.hpp>
#include <webots/Supervisor.hpp>
#include <webots/Field.hpp>
#include <webots/Servo.hpp>
#include <webots/DifferentialWheels.hpp>
#include <webots/DistanceSensor.hpp>
#include <webots/Camera.hpp>
#include <webots/Display.hpp>

#include <ros/ros.h>
#include <math.h>
//#include <string.hpp>
#include <stdio.h>
#include <std_msgs/String.h>
#include <sensor_msgs/LaserScan.h>
// %EndTag(MSG_HEADER)%

#include <sstream>
#include <iostream>
#include <fstream>


#include <iostream>
#include <string>
#include <cstring>
#include <sys/stat.h>

//#include "RosWebotsInterfaces/RWLaserServo.hpp"
//#include "RosWebotsInterfaces/RWRangeCamera.hpp"
//#include "RosWebotsInterfaces/RWKinect.hpp"
//#include "RosWebotsInterfaces/RWGPS.hpp"
//#include "RosWebotsInterfaces/RWCamera.hpp"

#include "RWSupervisorInterfaces/RWSDynObjs.hpp"



using namespace std;
using namespace webots;



class ConSUP : public Supervisor{
public:
  ConSUP();
  void run();
  ros::NodeHandle n;
  string folder_name;
protected:

  double time_step;

  webots::Supervisor* S;
 // RWSDynObjs RWSDO;




};

ConSUP::ConSUP() {
  printf("Comienza inicializacion\n");
  time_step = getBasicTimeStep();
  S = this;
  std::stringstream ss;
  ss<<"test";
 // RWSDO.init(S,&n,ss.str());

}

void ConSUP::run()
{
  //ACTIVE ROS PUBLICSHER
  printf("Run\n");

  std::stringstream ss;
  std_msgs::String msg;

  ros::Rate loop_rate(10);
  printf("Funciona?\n");
//  ROS_INFO("Funciona?");

  webots::Node* Root_node = S->getRoot();

  ofstream file;
  std::stringstream f_name;



  f_name << folder_name.c_str() << "/" << "webots_sup_locations"  << ".rosmat";

  string f_name2 = f_name.str();
  file.open(f_name2.c_str());



ros::Time t;
ros::Time t_start = ros::Time::now();
  while(true)
  {
	  step(time_step);
	  t = ros::Time::now();
	 ros::Duration d = t - t_start;
	  webots::Field* world_children = Root_node->getField("children");
	  int n = world_children->getCount(); //number of children
	  for (int i=0;i<n;i++)
	  {
		  webots::Node* n_aux = world_children->getMFNode(i);
		  int type = n_aux->getType();
		  if (type == webots::Node::DIFFERENTIAL_WHEELS || type == webots::Node::ROBOT)
		  {
			  webots::Field* f_name = n_aux->getField("name");
			  file << f_name->getSFString() << " ";
			  file << d.toSec() << " ";
			  const double* pos = n_aux->getPosition();
			  file << pos[0] << " " << pos[1] << " " << pos[2] << " ";
			  const double* ori = n_aux->getOrientation();
			  file << ori[0] << " " << ori[1] << " " << ori[2] << endl;
		  }
	  }

	  loop_rate.sleep();
  }
  file.close();

//  //GET RANGE MATRIX
//  double range_matrix[range_width][range_height];
//  range_image = RANGE->getRangeImage();
//  for(int i=0;i<range_width;i++)
//  {
//    for(int j = 0;j<range_height;j++){
//      distance = RANGE->rangeImageGetDepth(range_image,range_width,i,j);
//      if (distance > 79.9)
//        distance = 80;
//      range_matrix[i][j]=distance;
//    }



}


int main(int argc, char **argv)
{
  ros::init(argc, argv, "webotssupervisor");

  string folder_string = TrialFolderCreation();

  ConSUP *controller = new ConSUP();
  controller->folder_name = folder_string;
  controller->n.setParam("trial/folder_name",controller->folder_name);
  //Launch other nodes
  int roslaunch = fork();
  if(roslaunch == 0)
  {
    execlp("roslaunch","roslaunch","datmo.launch",NULL);
    return 0;
  }

  // launch the joy ROS node
    //  int roslaunch=fork();
    //  if (roslaunch==0) { // child process
    //   execlp("roslaunch","roslaunch","joy.launch",NULL);
    //   return 0;
    // }
  /*
   controller::step(1);
  ros::spinOnce();
        chatter_pub.publish(msg);
   controller::step(1);
  ros::spinOnce();
        chatter_pub.publish(msg);
   */
  controller->run();
  delete controller;
  ros::shutdown();
  //kill(roslaunch,SIGINT);
  return 0;
}


inline string TrialFolderCreation()
{
	//Time string for saving purposes
	time_t now = time(NULL);
	struct tm* now_time = gmtime(&now);
	std::stringstream time_str;
	time_str << now_time->tm_year+1900 <<"_" << now_time->tm_mon+1 <<"_";
	time_str << now_time->tm_mday <<"_" << now_time->tm_hour+2 <<"_";
	time_str << now_time->tm_min <<"_" << now_time->tm_sec;
	//folder creation
	std::stringstream folder_name;
	folder_name << "./trials/trial_" << time_str.str();

	string folder_string = folder_name.str();
	mkdir(folder_string.c_str(),S_IRWXU);
	return folder_string;
};
